/* Copyright (C) Dante Treglia II and Mark A. DeLoura, 2000. 
* All rights reserved worldwide.
*
* This software is provided "as is" without express or implied
* warranties. You may freely copy and compile this source into
* applications you distribute provided that the copyright text
* below is included in the resulting source code, for example:
* "Portions Copyright (C) Dante Treglia II and Mark A. DeLoura, 2000"
*/
//==========================================================
// C++ Matrix Library
// Version: 2.5
// Date: April 22, 2000
// Authors: Dante Treglia II and Mark A. DeLoura
// Thanks to: Miguel Gomez, Stan Melax, Pete Isensee, 
//   Gabor Nagy, Scott Bilas, James Boer, Eric Lengyel
//==========================================================
#include "mtxlib.h"
#include <cmath>
#include <cassert>

namespace mtxlib
{
	////////////////////////////////////////////////////////////
	// vector2 class
	//

	// Constructor with initializing float values
	vector2::vector2(float inX, float inY) 
	{
		x = inX;
		y = inY;
	}

	// Constructor with initializing vector2
	vector2::vector2(const vector2 &v) 
	{
		x = v.x;
		y = v.y;
	}

	// Array indexing
	float &vector2::operator [] (unsigned int i) 
	{
		assert (i<2);

		return *(&x+i);
	}

	// Array indexing
	const float &vector2::operator [] (unsigned int i) const 
	{
		assert (i<2);

		return *(&x+i);
	}

	// Assign 
	vector2 &vector2::operator = (const vector2 &v) 
	{
		x = v.x;
		y = v.y;

		return *this;
	}

	// Add a vector2 to this one
	vector2 &vector2::operator += (const vector2 &v) 
	{
		x += v.x;
		y += v.y;

		return *this;
	}

	// Subtract a vector2 from this one
	vector2 &vector2::operator -= (const vector2 &v) 
	{
		x -= v.x;
		y -= v.y;

		return *this;
	}

	// Multiply the vector2 by a float
	vector2 &vector2::operator *= (float f) 
	{
		x *= f;
		y *= f;

		return *this;
	}

	// Divide the vector2 by a float
	vector2 &vector2::operator /= (float f) 
	{
		x /= f;
		y /= f;

		return *this;
	}

	// Are these two vector2's equal?
	bool operator == (const vector2 &a, const vector2 &b) 
	{
		return ((a.x == b.x) && (a.y == b.y));
	}

	// Are these two vector2's not equal?
	bool operator != (const vector2 &a, const vector2 &b) 
	{
		return ((a.x != b.x) || (a.y != b.y));
	}

	// Add two vector2's
	vector2 operator + (const vector2 &a, const vector2 &b) 
	{
		vector2 ret(a);

		ret += b;

		return ret;
	}

	// Subtract one vector2 from another
	vector2 operator - (const vector2 &a, const vector2 &b) 
	{
		vector2 ret(a);

		ret -= b;

		return ret;
	}

	// Multiply vector2 by a float
	vector2 operator * (const vector2 &v, float f) 
	{
		return vector2(f * v.x, f * v.y);
	}

	// Multiply vector2 by a float
	vector2 operator * (float f, const vector2 &v) 
	{
		return vector2(f * v.x, f * v.y);
	}

	// Divide vector2 by a float
	vector2 operator / (const vector2 &v, float f) 
	{
		return vector2(v.x / f, v.y / f);
	}

	// Negate a vector2
	vector2 operator - (const vector2 &a) 
	{
		return vector2(-a.x, -a.y);
	}

	// Set Values
	void vector2::set(float xIn, float yIn)
	{
		x = xIn;
		y = yIn;
	}

	// Get length of a vector2
	float vector2::length() const 
	{
		return (float) sqrt(x*x + y*y);
	}

	// Get squared length of a vector2
	float vector2::lengthSqr() const 
	{
		return (x*x + y*y);
	}

	// Does vector2 equal (0, 0)?
	bool vector2::isZero() const 
	{
		return ((x == 0.0F) && (y == 0.0F));
	}

	// Normalize a vector2
	vector2 &vector2::normalize() 
	{
		float m = length();

		if (m > 0.0F) 
			m = 1.0F / m;
		else 
			m = 0.0F;
		x *= m;
		y *= m;

		return *this;
	}


	////////////////////////////////////////////////////////////
	// vector3 class
	//

	// Constructor with initializing float values
	vector3::vector3(float inX, float inY, float inZ) 
	{
		x = inX;
		y = inY;
		z = inZ;
	}

	// Constructor with initializing vector3
	vector3::vector3(const vector3 &v) 
	{
		x = v.x;
		y = v.y;
		z = v.z;
	}

	// Constructor with initializing vector2
	vector3::vector3(const vector2 &v) 
	{
		x = v.x;
		y = v.y;
		z = 0.0F;
	}

	// Constructor with initializing vector4
	vector3::vector3(const vector4 &v) 
	{
		x = v.x;
		y = v.y;
		z = v.z;
	}

	// Array indexing
	float &vector3::operator [] (unsigned int i) 
	{
		assert (i<3);

		return *(&x+i);
	}

	// Array indexing
	const float &vector3::operator [] (unsigned int i) const 
	{
		assert (i<3);

		return *(&x+i);
	}

	// Assign
	vector3 &vector3::operator = (const vector3 &v) 
	{
		x = v.x;
		y = v.y;
		z = v.z;

		return *this;
	}

	// Assign from a vector2
	vector3 &vector3::operator = (const vector2 &v) 
	{
		x = v.x;
		y = v.y;
		z = 0.0F;

		return *this;
	}

	// Add a vector3 to this one
	vector3 &vector3::operator += (const vector3 &v) 
	{
		x += v.x;
		y += v.y;
		z += v.z;

		return *this;
	}

	// Subtract a vector3 from this one
	vector3 &vector3::operator -= (const vector3 &v) 
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;

		return *this;
	}

	// Multiply the vector3 by a float
	vector3 &vector3::operator *= (float f) 
	{
		x *= f;
		y *= f;
		z *= f;

		return *this;
	}

	// Divide the vector3 by a float
	vector3 &vector3::operator /= (float f) 
	{
		x /= f;
		y /= f;
		z /= f;

		return *this;
	}

	// Are these two vector3's equal?
	bool operator == (const vector3 &a, const vector3 &b) 
	{
		return ((a.x == b.x) && (a.y == b.y) && (a.z == b.z));
	}

	// Are these two vector3's not equal?
	bool operator != (const vector3 &a, const vector3 &b) 
	{
		return ((a.x != b.x) || (a.y != b.y) || (a.z != b.z));
	}

	// Add two vector3's
	vector3 operator + (const vector3 &a, const vector3 &b) 
	{
		vector3 ret(a);

		ret += b;

		return ret;
	}

	// Subtract one vector3 from another
	vector3 operator - (const vector3 &a, const vector3 &b) 
	{
		vector3 ret(a);

		ret -= b;

		return ret;
	}

	// Multiply vector3 by a float
	vector3 operator * (const vector3 &v, float f) 
	{
		return vector3(f * v.x, f * v.y, f * v.z);
	}

	// Multiply vector3 by a float
	vector3 operator * (float f, const vector3 &v) 
	{
		return vector3(f * v.x, f * v.y, f * v.z);
	}

	// Divide vector3 by a float
	vector3 operator / (const vector3 &v, float f) 
	{
		return vector3(v.x / f, v.y / f, v.z / f);
	}

	// Negate a vector3
	vector3 operator - (const vector3 &a) 
	{
		return vector3(-a.x, -a.y, -a.z);
	}

	// Set Values
	void vector3::set(float xIn, float yIn, float zIn)
	{
		x = xIn;
		y = yIn;
		z = zIn;
	}

	// Get length of a vector3
	float vector3::length() const 
	{
		return (float) sqrt(x*x + y*y + z*z);
	}

	// Get squared length of a vector3
	float vector3::lengthSqr() const 
	{
		return (x*x + y*y + z*z);
	}

	// Does vector3 equal (0, 0, 0)?
	bool vector3::isZero() const 
	{
		return ((x == 0.0F) && (y == 0.0F) && (z == 0.0F));
	}

	// Normalize a vector3
	vector3 &vector3::normalize() 
	{
		float m = length();

		if (m > 0.0F) 
			m = 1.0F / m;
		else 
			m = 0.0F;
		x *= m;
		y *= m;
		z *= m;

		return *this;
	}


	////////////////////////////////////////////////////////////
	// vector4 class
	//

	// Constructor with initializing float values
	vector4::vector4(float inX, float inY, float inZ, float inW) 
	{
		x = inX;
		y = inY;
		z = inZ;
		w = inW;
	}

	// Constructor with initializing vector4
	vector4::vector4(const vector4 &v) 
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = v.w;
	}

	// Constructor with initializing vector3
	vector4::vector4(const vector3 &v) 
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = 0.0F;
	}

	// Constructor with initializing vector2
	vector4::vector4(const vector2 &v) 
	{
		x = v.x;
		y = v.y;
		z = 0.0F;
		w = 0.0F;
	}

	// Array indexing
	float &vector4::operator [] (unsigned int i) 
	{
		assert (i<4);

		return *(&x+i);
	}

	// Array indexing
	const float &vector4::operator [] (unsigned int i) const 
	{
		assert (i<4);

		return *(&x+i);
	}

	// Assign
	vector4 &vector4::operator = (const vector4 &v) 
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = v.w;

		return *this;
	}

	// Assign from a vector3
	vector4 &vector4::operator = (const vector3 &v) 
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = 0.0F;

		return *this;
	}

	// Assign from a vector2
	vector4 &vector4::operator = (const vector2 &v) 
	{
		x = v.x;
		y = v.y;
		z = 0.0F;
		w = 0.0F;

		return *this;
	}

	// Add a vector4 to this one
	vector4 &vector4::operator += (const vector4 &v) 
	{
		x += v.x;
		y += v.y;
		z += v.z;
		w += v.w;

		return *this;
	}

	// Subtract a vector4 from this one
	vector4 &vector4::operator -= (const vector4 &v) 
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;
		w -= v.w;

		return *this;
	}

	// Multiply the vector4 by a float
	vector4 &vector4::operator *= (float f) 
	{
		x *= f;
		y *= f;
		z *= f;
		w *= f;

		return *this;
	}

	// Divide the vector4 by a float
	vector4 &vector4::operator /= (float f) 
	{
		x /= f;
		y /= f;
		z /= f;
		w /= f;

		return *this;
	}

	// Are these two vector4's equal?
	bool operator == (const vector4 &a, const vector4 &b) 
	{
		return ((a.x == b.x) && (a.y == b.y) &&
			(a.z == b.z) && (a.w == b.w));
	}

	// Are these two vector4's not equal?
	bool operator != (const vector4 &a, const vector4 &b) 
	{
		return ((a.x != b.x) || (a.y != b.y) ||
			(a.z != b.z) || (a.w != b.w));
	}

	// Add two vector4's
	vector4 operator + (const vector4 &a, const vector4 &b) 
	{
		vector4 ret(a);

		ret += b;

		return ret;
	}

	// Subtract one vector4 from another
	vector4 operator - (const vector4 &a, const vector4 &b) 
	{
		vector4 ret(a);

		ret -= b;

		return ret;
	}

	// Multiply vector4 by a float
	vector4 operator * (const vector4 &v, float f) 
	{
		return vector4(f * v.x, f * v.y, f * v.z, f * v.w);
	}

	// Multiply vector4 by a float
	vector4 operator * (float f, const vector4 &v) 
	{
		return vector4(f * v.x, f * v.y, f * v.z, f * v.w);
	}

	// Divide vector4 by a float
	vector4 operator / (const vector4 &v, float f) 
	{
		return vector4(v.x / f, v.y / f, v.z / f, v.w / f);
	}

	// Negate a vector4
	vector4 operator - (const vector4 &a) 
	{
		return vector4(-a.x, -a.y, -a.z, -a.w);
	}

	// Set Values
	void vector4::set(float xIn, float yIn, float zIn, float wIn)
	{
		x = xIn;
		y = yIn;
		z = zIn;
		w = wIn;
	}

	// Get length of a vector4
	float vector4::length() const 
	{
		return (float) sqrt(x*x + y*y + z*z + w*w);
	}

	// Get squared length of a vector4
	float vector4::lengthSqr() const 
	{
		return (x*x + y*y + z*z + w*w);
	}

	// Does vector4 equal (0, 0, 0, 0)?
	bool vector4::isZero() const 
	{
		return ((x == 0.0F) && (y == 0.0F) && (z == 0.0F) && (w == 0.0F));
	}

	// Normalize a vector4
	vector4 &vector4::normalize() 
	{
		float m = length();

		if (m > 0.0F) 
			m = 1.0F / m;
		else 
			m = 0.0F;
		x *= m;
		y *= m;
		z *= m;
		w *= m;

		return *this;
	}
	// Normalize a vector4
	// Normalize x,y,z not w
	vector4 &vector4::normalize3() 
	{
		float m = length();

		if (m > 0.0F) 
			m = 1.0F / m;
		else 
			m = 0.0F;
		x *= m;
		y *= m;
		z *= m;

		return *this;
	}


	////////////////////////////////////////////////////////////
	// Miscellaneous vector functions
	//

	// Return Normal of vector2's
	vector2 Normalized(const vector2 &a)
	{
		vector2 ret(a);
		return ret.normalize();
	}
	// Return Normal of vector3's
	vector3 Normalized(const vector3 &a)
	{
		vector3 ret(a);
		return ret.normalize();
	}
	// Return Normal of vector4's
	vector4 Normalized(const vector4 &a)
	{
		vector4 ret(a);
		return ret.normalize();
	}
	// Return Normal of vector4's
	// x,y,z not w
	vector4 Normalized3(const vector4 &a)
	{
		vector4 ret(a);
		return ret.normalize();
	}

	// Dot product of two vector2's
	float DotProduct(const vector2 &a, const vector2 &b) 
	{
		return a.x*b.x + a.y*b.y;
	}

	// Dot product of two vector3's
	float DotProduct(const vector3 &a, const vector3 &b) 
	{
		return a.x*b.x + a.y*b.y + a.z*b.z;
	}

	// Dot product of two vector4's
	float DotProduct(const vector4 &a, const vector4 &b) 
	{
		return a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w;
	}

	// Dot product of two vector4's
	// x,y,z not w
	float DotProduct3(const vector4 &a, const vector4 &b) 
	{
		return a.x*b.x + a.y*b.y + a.z*b.z;
	}
	// Swap two vector2's
	void SwapVec(vector2 &a, vector2 &b) 
	{
		vector2 tmp(a);

		a = b;
		b = tmp;
	}

	// Swap two vector3's
	void SwapVec(vector3 &a, vector3 &b) 
	{
		vector3 tmp(a);

		a = b;
		b = tmp;
	}

	// Swap two vector4's
	void SwapVec(vector4 &a, vector4 &b) 
	{
		vector4 tmp(a);

		a = b;
		b = tmp;
	}

	// Cross product of two vector3's
	vector3 CrossProduct(const vector3 &a, const vector3 &b) 
	{
		return vector3(a.y*b.z - a.z*b.y,
			a.z*b.x - a.x*b.z,
			a.x*b.y - a.y*b.x);
	}
	// Cross product of two vector4's
	// x,y,z not w
	vector4 CrossProduct3(const vector4 &a, const vector4 &b) 
	{
		return vector4(a.y*b.z - a.z*b.y,
			a.z*b.x - a.x*b.z,
			a.x*b.y - a.y*b.x,0);
	}

	// Are these two vector2's nearly equal?
	bool NearlyEquals( const vector2& a, const vector2& b, float r ) 
	{
		vector2 diff = a - b; // difference

		return (DotProduct(diff, diff) < r*r); // radius
	}

	// Are these two vector3's nearly equal?
	bool NearlyEquals( const vector3& a, const vector3& b, float r ) 
	{
		vector3 diff = a - b; // difference

		return (DotProduct(diff, diff) < r*r); // radius
	}

	// Are these two vector4's nearly equal?
	bool NearlyEquals( const vector4& a, const vector4& b, float r ) 
	{
		vector4 diff = a - b; // difference

		return (DotProduct(diff, diff) < r*r); // radius
	}


	////////////////////////////////////////////////////////////
	// matrix33 class
	//

	// Constructor with initializing value
	matrix33::matrix33(float v) 
	{
		col[0].set(v, v, v);
		col[1].set(v, v, v);
		col[2].set(v, v, v);
	}

	// Constructor with initializing matrix33
	matrix33::matrix33(const matrix33 &m) 
	{
		col[0] = m[0];
		col[1] = m[1];
		col[2] = m[2];
	}

	// Constructor with initializing vector3's
	matrix33::matrix33(const vector3 &v0, const vector3 &v1, const vector3 &v2) 
	{
		col[0] = v0;
		col[1] = v1;
		col[2] = v2;
	}

	// Array indexing
	vector3 &matrix33::operator [] (unsigned int i) 
	{
		assert (i<3);

		return (vector3&)col[i];
	}

	// Array indexing
	const vector3 &matrix33::operator [] (unsigned int i) const 
	{
		assert (i<3);

		return (vector3&)col[i];
	}

	// Assign
	matrix33 &matrix33::operator = (const matrix33 &m) 
	{
		col[0] = m[0];
		col[1] = m[1];
		col[2] = m[2];

		return *this;
	}

	// Add a matrix33 to this one
	matrix33 &matrix33::operator += (const matrix33 &m) 
	{
		col[0] += m[0];
		col[1] += m[1];
		col[2] += m[2];

		return *this;
	}

	// Subtract a matrix33 from this one
	matrix33 &matrix33::operator -= (const matrix33 &m) 
	{
		col[0] -= m[0];
		col[1] -= m[1];
		col[2] -= m[2];

		return *this;
	}

	// Multiply the matrix33 by another matrix33
	matrix33 &matrix33::operator *= (const matrix33 &m) 
	{
		matrix33 t;

		for (unsigned int r = 0; r < 3; r++) 
		{
			for (unsigned int c = 0; c < 3; c++) 
			{
				float f = 0;

				f += col[0][r] * m[c][0];
				f += col[1][r] * m[c][1];
				f += col[2][r] * m[c][2];

				t[c][r] = f;
			}
		}

		*this = t;

		return *this;
	}

	// Multiply the matrix33 by a float
	matrix33 &matrix33::operator *= (float f) 
	{
		col[0] *= f;
		col[1] *= f;
		col[2] *= f;

		return *this;
	}

	// Are these two matrix33's equal?
	bool operator == (const matrix33 &a, const matrix33 &b) 
	{
		return ((a[0] == b[0]) && (a[1] == b[1]) && (a[2] == b[2]));
	}

	// Are these two matrix33's not equal?
	bool operator != (const matrix33 &a, const matrix33 &b) 
	{
		return ((a[0] != b[0]) || (a[1] != b[1]) || (a[2] != b[2]));
	}

	// Add two matrix33's
	matrix33 operator + (const matrix33 &a, const matrix33 &b) 
	{
		matrix33 ret(a);

		ret += b;

		return ret;
	}

	// Subtract one matrix33 from another
	matrix33 operator - (const matrix33 &a, const matrix33 &b) 
	{
		matrix33 ret(a);

		ret -= b;

		return ret;
	}

	// Multiply matrix33 by another matrix33
	matrix33 operator * (const matrix33 &a, const matrix33 &b) 
	{
		matrix33 ret(a);

		ret *= b;

		return ret;
	}

	// Multiply a vector3 by this matrix33
	vector3 operator * (const matrix33 &m, const vector3 &v) 
	{
		vector3 ret;

		ret.x = v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0];
		ret.y = v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1];
		ret.z = v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2];

		return ret;
	}

	// Multiply a vector3 by this matrix33
	vector3 operator * (const vector3 &v, const matrix33 &m) 
	{
		vector3 ret;

		ret.x = DotProduct(m[0], v);
		ret.y = DotProduct(m[1], v);
		ret.z = DotProduct(m[2], v);

		return ret;

	}

	// Multiply matrix33 by a float
	matrix33 operator * (float f, const matrix33 &m) 
	{
		matrix33 ret(m);

		ret *= f;

		return ret;
	}

	// Multiply matrix33 by a float
	matrix33 operator * (const matrix33 &m, float f) 
	{
		matrix33 ret(m);

		ret *= f;

		return ret;
	}

	// Set matrix33 to the identity matrix
	matrix33 &matrix33::identity() 
	{
		for (unsigned int c = 0; c < 3; c++) 
		{
			for (unsigned int r = 0; r < 3; r++) 
			{
				if (c == r) 
					col[c][r] = 1.0F;
				else 
					col[c][r] = 0.0F;
			} 
		}

		return *this;
	}

	// Transpose the matrix33
	matrix33 &matrix33::transpose() 
	{
		float t;

		for (unsigned int c = 0; c < 3; c++) 
		{
			for (unsigned int r = c + 1; r < 3; r++) 
			{
				t = col[c][r];
				col[c][r] = col[r][c];
				col[r][c] = t;
			} 
		}

		return *this;
	}

	// Invert the matrix33
	matrix33 &matrix33::invert() 
	{
		matrix33 a(*this);
		matrix33 b(IdentityMatrix33());

		unsigned int c, r;
		unsigned int cc;
		unsigned int rowMax; // Points to max abs value row in this column
		unsigned int row;
		float tmp;

		// Go through columns
		for (c=0; c<3; c++) 
		{
			// Find the row with max value in this column
			rowMax = c;
			for (r=c+1; r<3; r++) 
			{
				if (fabs(a[c][r]) > fabs(a[c][rowMax])) 
				{
					rowMax = r;
				}
			}

			// If the max value here is 0, we can't invert.  Return identity.
			if (a[rowMax][c] == 0.0F) 
				return (identity());

			// Swap row "rowMax" with row "c"
			for (cc=0; cc<3; cc++) 
			{
				tmp = a[cc][c];
				a[cc][c] = a[cc][rowMax];
				a[cc][rowMax] = tmp;
				tmp = b[cc][c];
				b[cc][c] = b[cc][rowMax];
				b[cc][rowMax] = tmp;
			}

			// Now everything we do is on row "c".
			// Set the max cell to 1 by dividing the entire row by that value
			tmp = a[c][c];
			for (cc=0; cc<3; cc++) 
			{
				a[cc][c] /= tmp;
				b[cc][c] /= tmp;
			}

			// Now do the other rows, so that this column only has a 1 and 0's
			for (row = 0; row < 3; row++) 
			{
				if (row != c) 
				{
					tmp = a[c][row];
					for (cc=0; cc<3; cc++) 
					{
						a[cc][row] -= a[cc][c] * tmp;
						b[cc][row] -= b[cc][c] * tmp;
					}
				}
			}

		}

		*this = b;

		return *this;
	}

	// Return a matrix33 set to the identity matrix
	matrix33 IdentityMatrix33() 
	{
		matrix33 ret;

		return ret.identity();
	}

	// Return the transpose of the matrix33
	matrix33 TransposeMatrix33(const matrix33 &m) 
	{
		matrix33 ret(m);

		return ret.transpose();
	}

	// Return the inverted matrix33
	matrix33 InvertMatrix33(const matrix33 &m) 
	{
		matrix33 ret(m);

		return ret.invert();
	}

	// Return a 2D rotation matrix33
	matrix33 RotateRadMatrix33(float rad) 
	{
		matrix33 ret;
		float sinA, cosA;

		sinA = (float)sin(rad);
		cosA = (float)cos(rad);

		ret[0][0] = cosA; ret[1][0] = -sinA; ret[2][0] = 0.0F;
		ret[0][1] = sinA; ret[1][1] =  cosA; ret[2][1] = 0.0F;
		ret[0][2] = 0.0F; ret[1][2] =  0.0F; ret[2][2] = 1.0F;

		return ret;
	}

	// Return a 2D translation matrix33
	matrix33 TranslateMatrix33(float x, float y) 
	{
		matrix33 ret;

		ret.identity();
		ret[2][0] = x;
		ret[2][1] = y;

		return ret;
	}

	// Return a 2D/3D scale matrix33
	matrix33 ScaleMatrix33(float x, float y, float z) 
	{
		matrix33 ret;

		ret.identity();
		ret[0][0] = x;
		ret[1][1] = y;
		ret[2][2] = z;

		return ret;
	}


	////////////////////////////////////////////////////////////
	// matrix44 class
	//

	// Constructor with initializing value
	matrix44::matrix44(float v) 
	{
		col[0].set(v, v, v, v);
		col[1].set(v, v, v, v);
		col[2].set(v, v, v, v);
		col[3].set(v, v, v, v);
	}

	// Constructor with initializing matrix44
	matrix44::matrix44(const matrix44 &m) 
	{
		col[0] = m[0];
		col[1] = m[1];
		col[2] = m[2];
		col[3] = m[3];
	}

	// Constructor with initializing matrix33
	matrix44::matrix44(const matrix33 &m) 
	{
		col[0] = m[0];
		col[1] = m[1];
		col[2] = m[2];
		col[3] = vector4(0.0, 0.0, 0.0, 1.0);
	}

	// Constructor with initializing vector4's
	matrix44::matrix44(const vector4 &v0, const vector4 &v1, 
		const vector4 &v2, const vector4 &v3) 
	{
		col[0] = v0;
		col[1] = v1;
		col[2] = v2;
		col[3] = v3;
	}

	// Array indexing
	vector4 &matrix44::operator [] (unsigned int i) 
	{
		assert (i<4);

		return (vector4&) col[i];
	}

	// Array indexing
	const vector4 &matrix44::operator [] (unsigned int i) const 
	{
		assert (i<4);

		return (vector4&) col[i];
	}

	// Assign
	matrix44 &matrix44::operator = (const matrix44 &m) 
	{
		col[0] = m[0];
		col[1] = m[1];
		col[2] = m[2];
		col[3] = m[3];

		return *this;
	}

	// Assign a matrix33 to the matrix44
	matrix44 &matrix44::operator = (const matrix33 &m) 
	{
		col[0] = m[0];
		col[1] = m[1];
		col[2] = m[2];
		col[3] = vector4(0.0, 0.0, 0.0, 1.0);

		return *this;
	}

	// Add a matrix44 to this one
	matrix44 &matrix44::operator += (const matrix44 &m) 
	{
		col[0] += m[0];
		col[1] += m[1];
		col[2] += m[2];
		col[3] += m[3];

		return *this;
	}

	// Subtract a matrix44 from this one
	matrix44 &matrix44::operator -= (const matrix44 &m) 
	{
		col[0] -= m[0];
		col[1] -= m[1];
		col[2] -= m[2];
		col[3] -= m[3];

		return *this;
	}

	// Multiply the matrix44 by another matrix44
	matrix44 &matrix44::operator *= (const matrix44 &m) 
	{
		matrix44 t;

		for (unsigned int r = 0; r < 4; r++) 
		{
			for (unsigned int c = 0; c < 4; c++) 
			{
				float f = 0;

				f += col[0][r] * m[c][0];
				f += col[1][r] * m[c][1];
				f += col[2][r] * m[c][2];
				f += col[3][r] * m[c][3];

				t[c][r] = f;
			}
		}

		*this = t;

		return *this;
	}

	// Multiply the matrix44 by a float
	matrix44 &matrix44::operator *= (float f) 
	{
		col[0] *= f;
		col[1] *= f;
		col[2] *= f;
		col[3] *= f;

		return *this;
	}

	// Are these two matrix44's equal?
	bool operator == (const matrix44 &a, const matrix44 &b) 
	{
		return ((a[0] == b[0]) && (a[1] == b[1]) &&
			(a[2] == b[2]) && (a[3] == b[3]));
	}

	// Are these two matrix44's not equal?
	bool operator != (const matrix44 &a, const matrix44 &b) 
	{
		return ((a[0] != b[0]) || (a[1] != b[1]) ||
			(a[2] != b[2]) || (a[3] != b[3]));
	}

	// Add two matrix44's
	matrix44 operator + (const matrix44 &a, const matrix44 &b) 
	{
		matrix44 ret(a);

		ret += b;

		return ret;
	}

	// Subtract one matrix44 from another
	matrix44 operator - (const matrix44 &a, const matrix44 &b) 
	{
		matrix44 ret(a);

		ret -= b;

		return ret;
	}

	// Multiply matrix44 by another matrix44
	matrix44 operator * (const matrix44 &a, const matrix44 &b) 
	{
		matrix44 ret(a);

		ret *= b;

		return ret;
	}

	// Multiply a vector3 by this matrix44
	vector3 operator * (const matrix44 &m, const vector3 &v) 
	{
		vector4 ret(v);

		ret = m * ret;

		return vector3(ret.x, ret.y, ret.z);
	}

	// Multiply a vector3 by this matrix44
	vector3 operator * (const vector3 &v, const matrix44 &m) 
	{
		vector4 ret(v);

		ret = ret * m;

		return vector3(ret.x, ret.y, ret.z);
	}

	// Multiply a vector4 by this matrix44
	vector4 operator * (const matrix44 &m, const vector4 &v) 
	{
		vector4 ret;

		ret.x = v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0] + v.w * m[3][0];
		ret.y = v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1] + v.w * m[3][1];
		ret.z = v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2] + v.w * m[3][2];
		ret.w = v.x * m[0][3] + v.y * m[1][3] + v.z * m[2][3] + v.w * m[3][3];

		return ret;
	}

	// Multiply a vector4 by this matrix44
	vector4 operator * (const vector4 &v, const matrix44 &m) 
	{
		vector4 ret;

		ret.x = DotProduct(m[0], v);
		ret.y = DotProduct(m[1], v);
		ret.z = DotProduct(m[2], v);
		ret.w = DotProduct(m[3], v);

		return ret;
	}

	// Multiply matrix44 by a float
	matrix44 operator * (float f, const matrix44 &m) 
	{
		matrix44 ret(m);

		ret *= f;

		return ret;
	}

	// Set matrix44 to the identity matrix
	matrix44 &matrix44::identity() 
	{
		for (unsigned int c = 0; c < 4; c++) 
		{
			for (unsigned int r = 0; r < 4; r++) 
			{
				if (c == r) 
					col[c][r] = 1.0F;
				else 
					col[c][r] = 0.0F;
			} 
		} 

		return *this;
	}

	// Transpose the matrix44
	matrix44 &matrix44::transpose() 
	{
		float t;

		for (unsigned int c = 0; c < 4; c++) 
		{
			for (unsigned int r = c + 1; r < 4; r++) 
			{
				t = col[c][r];
				col[c][r] = col[r][c];
				col[r][c] = t;
			} 
		} 

		return *this;
	}

	// Invert the matrix44
	matrix44 &matrix44::invert() 
	{
		matrix44 a(*this);
		matrix44 b(IdentityMatrix44());

		unsigned int r, c;
		unsigned int cc;
		unsigned int rowMax; // Points to max abs value row in this column
		unsigned int row;
		float tmp;

		// Go through columns
		for (c=0; c<4; c++) 
		{

			// Find the row with max value in this column
			rowMax = c;
			for (r=c+1; r<4; r++) 
			{
				if (fabs(a[c][r]) > fabs(a[c][rowMax])) 
				{
					rowMax = r;
				}
			}

			// If the max value here is 0, we can't invert.  Return identity.
			if (a[rowMax][c] == 0.0F) 
				return (identity());

			// Swap row "rowMax" with row "c"
			for (cc=0; cc<4; cc++) 
			{
				tmp = a[cc][c];
				a[cc][c] = a[cc][rowMax];
				a[cc][rowMax] = tmp;
				tmp = b[cc][c];
				b[cc][c] = b[cc][rowMax];
				b[cc][rowMax] = tmp;
			}

			// Now everything we do is on row "c".
			// Set the max cell to 1 by dividing the entire row by that value
			tmp = a[c][c];
			for (cc=0; cc<4; cc++) 
			{
				a[cc][c] /= tmp;
				b[cc][c] /= tmp;
			}

			// Now do the other rows, so that this column only has a 1 and 0's
			for (row = 0; row < 4; row++) 
			{
				if (row != c) 
				{
					tmp = a[c][row];
					for (cc=0; cc<4; cc++) 
					{
						a[cc][row] -= a[cc][c] * tmp;
						b[cc][row] -= b[cc][c] * tmp;
					}
				}
			}

		}

		*this = b;

		return *this;
	}

	// Return a matrix44 set to the identity matrix
	matrix44 IdentityMatrix44() 
	{
		matrix44 ret;

		return ret.identity();
	}

	// Return the transpose of the matrix44
	matrix44 TransposeMatrix44(const matrix44 &m) 
	{
		matrix44 ret(m);

		return ret.transpose();
	}

	// Return the inverted matrix44
	matrix44 InvertMatrix44(const matrix44 &m) 
	{
		matrix44 ret(m);

		return ret.invert();
	}

	// Return a 3D axis-rotation matrix44
	// Pass in 'x', 'y', or 'z' for the axis.
	matrix44 RotateRadMatrix44(char axis, float rad) 
	{
		matrix44 ret;
		float sinA, cosA;

		sinA = (float)sin(rad);
		cosA = (float)cos(rad);

		switch(axis) 
		{
		case 'x':
		case 'X':
			ret[0][0] =  1.0F; ret[1][0] =  0.0F; ret[2][0] =  0.0F;
			ret[0][1] =  0.0F; ret[1][1] =  cosA; ret[2][1] = -sinA;
			ret[0][2] =  0.0F; ret[1][2] =  sinA; ret[2][2] =  cosA;
			break;

		case 'y':
		case 'Y':
			ret[0][0] =  cosA; ret[1][0] =  0.0F; ret[2][0] =  sinA;
			ret[0][1] =  0.0F; ret[1][1] =  1.0F; ret[2][1] =  0.0F;
			ret[0][2] = -sinA; ret[1][2] =  0.0F; ret[2][2] =  cosA;
			break;

		case 'z':
		case 'Z':
			ret[0][0] =  cosA; ret[1][0] = -sinA; ret[2][0] =  0.0F;
			ret[0][1] =  sinA; ret[1][1] =  cosA; ret[2][1] =  0.0F;
			ret[0][2] =  0.0F; ret[1][2] =  0.0F; ret[2][2] =  1.0F;
			break;
		}

		ret[0][3] = 0.0F; ret[1][3] = 0.0F; ret[2][3] = 0.0F;
		ret[3][0] = 0.0F;
		ret[3][1] = 0.0F;
		ret[3][2] = 0.0F;
		ret[3][3] = 1.0F;

		return ret;
	}

	// Return a 3D axis-rotation matrix44
	// Pass in an arbitrary vector3 axis.
	matrix44 RotateRadMatrix44(const vector3 &axis, float rad) 
	{
		matrix44 ret;
		float sinA, cosA;
		float invCosA;
		vector3 nrm = axis;
		float x, y, z;
		float xSq, ySq, zSq;

		nrm.normalize();
		sinA = (float)sin(rad);
		cosA = (float)cos(rad);
		invCosA = 1.0F - cosA;

		x = nrm.x;
		y = nrm.y;
		z = nrm.z;

		xSq = x * x;
		ySq = y * y;
		zSq = z * z;

		ret[0][0] = (invCosA * xSq) + (cosA);
		ret[1][0] = (invCosA * x * y) - (sinA * z );
		ret[2][0] = (invCosA * x * z) + (sinA * y );
		ret[3][0] = 0.0F;

		ret[0][1] = (invCosA * x * y) + (sinA * z);
		ret[1][1] = (invCosA * ySq) + (cosA);
		ret[2][1] = (invCosA * y * z) - (sinA * x);
		ret[3][1] = 0.0F;

		ret[0][2] = (invCosA * x * z) - (sinA * y);
		ret[1][2] = (invCosA * y * z) + (sinA * x);
		ret[2][2] = (invCosA * zSq) + (cosA);
		ret[3][2] = 0.0F;

		ret[0][3] = 0.0F;
		ret[1][3] = 0.0F;
		ret[2][3] = 0.0F;
		ret[3][3] = 1.0F;

		return ret;
	}

	// Return a 3D translation matrix44
	matrix44 TranslateMatrix44(float x, float y, float z) 
	{
		matrix44 ret;

		ret.identity();
		ret[3][0] = x;
		ret[3][1] = y;
		ret[3][2] = z;

		return ret;
	}

	// Return a 3D/4D scale matrix44
	matrix44 ScaleMatrix44(float x, float y, float z, float w) 
	{
		matrix44 ret;

		ret.identity();
		ret[0][0] = x;
		ret[1][1] = y;
		ret[2][2] = z;
		ret[3][3] = w;

		return ret;
	}

	// Return a "lookat" matrix44 given the current camera position (vector3),
	//   camera-up vector3, and camera-target vector3.
	matrix44 LookAtMatrix44(const vector3 &camPos, const vector3 &camUp, 
		const vector3 &target ) 
	{
		matrix44 rot;
		matrix44 tran;

		vector3 look = (camPos - target);
		look.normalize();

		vector3 right = CrossProduct(camUp, look);
		right.normalize();

		vector3 up = CrossProduct(look, right);
		up.normalize();

		rot[0][0] = right.x;
		rot[1][0] = right.y;
		rot[2][0] = right.z;
		rot[3][0] = 0.0;

		rot[0][1] = up.x;
		rot[1][1] = up.y;
		rot[2][1] = up.z;
		rot[3][1] = 0.0;

		rot[0][2] = look.x;
		rot[1][2] = look.y;
		rot[2][2] = look.z;
		rot[3][2] = 0.0;

		rot[0][3] = 0.0F;
		rot[1][3] = 0.0F;
		rot[2][3] = 0.0F;
		rot[3][3] = 1.0F;

		tran = TranslateMatrix44(-camPos.x, -camPos.y, -camPos.z);

		return (rot * tran);
	}

	// Return a frustum matrix44 given the left, right, bottom, top,
	//   near, and far values for the frustum boundaries.
	matrix44 FrustumMatrix44(float l, float r, 
		float b, float t, float n, float f) 
	{
		matrix44 ret;
		float width = r-l;
		float height = t-b;
		float depth = f-n;

		ret[0][0] = (2*n) / width;
		ret[0][1] = 0.0F;
		ret[0][2] = 0.0F;
		ret[0][3] = 0.0F;

		ret[1][0] = 0.0F;
		ret[1][1] = (2*n) / height;
		ret[1][2] = 0.0F;
		ret[1][3] = 0.0F;

		ret[2][0] = (r + l) / width;
		ret[2][1] = (t + b) / height;
		ret[2][2] = -(f + n) / depth;
		ret[2][3] = -1.0F;

		ret[3][0] = 0.0F;
		ret[3][1] = 0.0F;
		ret[3][2] = -(2*f*n) / depth;
		ret[3][3] = 0.0F;

		return ret;
	}

	// Return a perspective matrix44 given the field-of-view in the Y
	//   direction in degrees, the aspect ratio of Y/X, and near and
	//   far plane distances.
	matrix44 PerspectiveMatrix44(float fovY, float aspect, float n, float f) 
	{
		matrix44 ret;
		float angle;
		float cot;

		angle = fovY / 2.0F;
		angle = DegToRad( angle );

		cot = (float) cos(angle) / (float) sin(angle);

		ret[0][0] = cot / aspect;
		ret[0][1] = 0.0F;
		ret[0][2] = 0.0F;
		ret[0][3] = 0.0F;

		ret[1][0] = 0.0F;
		ret[1][1] = cot;
		ret[1][2] = 0.0F;
		ret[1][3] = 0.0F;

		ret[2][0] = 0.0F;
		ret[2][1] = 0.0F;
		ret[2][2] = -(f + n) / (f - n);
		ret[2][3] = -1.0F;


		ret[3][0] = 0.0F;
		ret[3][1] = 0.0F;
		ret[3][2] = -(2*f*n) / (f - n);
		ret[3][3] = 0.0F;

		return ret;
	}

	// Return an orthographic matrix44 given the left, right, bottom, top,
	//   near, and far values for the frustum boundaries.
	matrix44 OrthoMatrix44(float l, float r, 
		float b, float t, float n, float f) 
	{
		matrix44 ret;
		float width = r-l;
		float height = t-b;
		float depth = f-n;

		ret[0][0] = 2.0F / width;
		ret[0][1] = 0.0F;
		ret[0][2] = 0.0F;
		ret[0][3] = 0.0F;

		ret[1][0] = 0.0F;
		ret[1][1] = 2.0F / height;
		ret[1][2] = 0.0F;
		ret[1][3] = 0.0F;

		ret[2][0] = 0.0F;
		ret[2][1] = 0.0F;
		ret[2][2] = -(2.0F) / depth;
		ret[2][3] = 0.0F;

		ret[3][0] = -(r + l) / width;
		ret[3][1] = -(t + b) / height;
		ret[3][2] = -(f + n) / depth;
		ret[3][3] = 1.0F;

		return ret;
	}

	// Return an orientation matrix using 3 basis normalized vectors
	matrix44	OrthoNormalMatrix44(const vector3 &xdir, 
		const vector3 &ydir, const vector3 &zdir)
	{
		matrix44 ret;

		ret[0] = (vector4)xdir;
		ret[1] = (vector4)ydir;
		ret[2] = (vector4)zdir;
		ret[3][3] = 1.0F;

		return ret;
	}


	////////////////////////////////////////////////////////////
	// Debug functions
	//

	// Print a vector2 to a file
	void vector2::fprint(FILE* file, char* str) const 
	{
		fprintf(file, "%svector2: <%f, %f>\n", str, x, y);
	}

	// Print a vector3 to a file
	void vector3::fprint(FILE* file, char* str) const 
	{
		fprintf(file, "%svector3: <%f, %f, %f>\n", str, x, y, z);
	}

	// Print a vector4 to a file
	void vector4::fprint(FILE* file, char* str) const 
	{
		fprintf(file, "%svector4: <%f, %f, %f, %f>\n", str, x, y, z, w);
	}

	// Print a matrix33 to a file
	void matrix33::fprint(FILE* file, char * str) const 
	{
		fprintf(file, "%smatrix33:\n", str);
		vector3 row0(col[0][0], col[1][0], col[2][0]);
		row0.fprint(file, "\t");
		vector3 row1(col[0][1], col[1][1], col[2][1]);
		row1.fprint(file, "\t");
		vector3 row2(col[0][2], col[1][2], col[2][2]);
		row2.fprint(file, "\t");
	}

	// Print a matrix44 to a file
	void matrix44::fprint(FILE* file, char* str) const 
	{
		fprintf(file, "%smatrix44:\n", str);
		vector4 row0(col[0][0], col[1][0], col[2][0], col[3][0]);
		row0.fprint(file, "\t");
		vector4 row1(col[0][1], col[1][1], col[2][1], col[3][1]);
		row1.fprint(file, "\t");
		vector4 row2(col[0][2], col[1][2], col[2][2], col[3][2]);
		row2.fprint(file, "\t");
		vector4 row3(col[0][3], col[1][3], col[2][3], col[3][3]);
		row3.fprint(file, "\t");
	}
//===========================================================

	vector4 matrix44::quaternion() const {
		const matrix44 &_this = *this;
		float trace = _this[0][0] + _this[1][1] + _this[2][2] + 1.0f;
		vector4 q(0, 0, 0, 0);
		const float M_EPSILON = 0.001;
		if( trace > M_EPSILON ) {
			float s = 0.5f / sqrtf(trace);
			q.w = 0.25f / s;
			q.x = ( _this[1][2] - _this[2][1] ) * s;
			q.y = ( _this[2][0] - _this[0][2] ) * s;
			q.z = ( _this[0][1] - _this[1][0] ) * s;
		}
		else {
			if ( _this[0][0] > _this[1][1] && _this[0][0] > _this[2][2] ) {
				float s = 2.0f * sqrtf( 1.0f + _this[0][0] - _this[1][1] - _this[2][2]);
				q.w = (_this[2][1] - _this[1][2] ) / s;
				q.x = 0.25f * s;
				q.y = (_this[1][0] + _this[0][1] ) / s;
				q.z = (_this[2][0] + _this[0][2] ) / s;
			} else if (_this[1][1] > _this[2][2]) {
				float s = 2.0f * sqrtf( 1.0f + _this[1][1] - _this[0][0] - _this[2][2]);
				q.w = (_this[2][0] - _this[0][2] ) / s;
				q.x = (_this[1][0] + _this[0][1] ) / s;
				q.y = 0.25f * s;
				q.z = (_this[2][1] + _this[1][2] ) / s;
			} else {
				float s = 2.0f * sqrtf( 1.0f + _this[2][2] - _this[0][0] - _this[1][1] );
				q.w = (_this[1][0] - _this[0][1] ) / s;
				q.x = (_this[2][0] + _this[0][2] ) / s;
				q.y = (_this[2][1] + _this[1][2] ) / s;
				q.z = 0.25f * s;
			}
		}

		return q;
	}
	
	vector4 matrix33::quaternion() const {
		const matrix33 &_this = *this;
		float trace = _this[0][0] + _this[1][1] + _this[2][2] + 1.0f;
		vector4 q(0, 0, 0, 0);
		const float M_EPSILON = 0.001;
		if( trace > M_EPSILON ) {
			float s = 0.5f / sqrtf(trace);
			q.w = 0.25f / s;
			q.x = ( _this[1][2] - _this[2][1] ) * s;
			q.y = ( _this[2][0] - _this[0][2] ) * s;
			q.z = ( _this[0][1] - _this[1][0] ) * s;
		}
		else {
			if ( _this[0][0] > _this[1][1] && _this[0][0] > _this[2][2] ) {
				float s = 2.0f * sqrtf( 1.0f + _this[0][0] - _this[1][1] - _this[2][2]);
				q.w = (_this[2][1] - _this[1][2] ) / s;
				q.x = 0.25f * s;
				q.y = (_this[1][0] + _this[0][1] ) / s;
				q.z = (_this[2][0] + _this[0][2] ) / s;
			} else if (_this[1][1] > _this[2][2]) {
				float s = 2.0f * sqrtf( 1.0f + _this[1][1] - _this[0][0] - _this[2][2]);
				q.w = (_this[2][0] - _this[0][2] ) / s;
				q.x = (_this[1][0] + _this[0][1] ) / s;
				q.y = 0.25f * s;
				q.z = (_this[2][1] + _this[1][2] ) / s;
			} else {
				float s = 2.0f * sqrtf( 1.0f + _this[2][2] - _this[0][0] - _this[1][1] );
				q.w = (_this[1][0] - _this[0][1] ) / s;
				q.x = (_this[2][0] + _this[0][2] ) / s;
				q.y = (_this[2][1] + _this[1][2] ) / s;
				q.z = 0.25f * s;
			}
		}
		return q;
	}


	void matrix33::printf() const
	{
		for(int i = 0; i < 3; i++)
		{
			for(int j = 0; j < 3; j++)
			{
				::printf("\%5.2f ", (*this)[j][i]);
			}
			::printf("\n");
		}
	}


	void matrix44::printf() const 
	{
		for(int i = 0; i < 4; i++)
		{
			for(int j = 0; j < 4; j++)
			{
				::printf("%5.2f ", (*this)[j][i]);
			}
			::printf("\n");
		}
	}


	void vector2::printf() const 
	{
		::printf("%5.2f %5.2f\n", (*this)[0], (*this)[1]);
	}


	void vector3::printf() const 
	{
		::printf("%5.2f %5.2f %5.2f\n", (*this)[0], (*this)[1], (*this)[2]);
	}


	void vector4::printf() const 
	{
		::printf("%5.2f %5.2f %5.2f %5.2f\n", (*this)[0], (*this)[1], (*this)[2], (*this)[3]);
	}


	matrix33 &matrix33::invert(bool &isInvertible)
	{
		matrix33 a(*this);
		matrix33 b(IdentityMatrix33());

		unsigned int c, r;
		unsigned int cc;
		unsigned int rowMax; // Points to max abs value row in this column
		unsigned int row;
		float tmp;

		// Go through columns
		for (c=0; c<3; c++) 
		{
			// Find the row with max value in this column
			rowMax = c;
			for (r=c+1; r<3; r++) 
			{
				if (fabs(a[c][r]) > fabs(a[c][rowMax])) 
				{
					rowMax = r;
				}
			}

			// If the max value here is 0, we can't invert.  Return identity.
			if (a[rowMax][c] == 0.0F)
			{
				isInvertible = false;
				return (identity());
			}

			// Swap row "rowMax" with row "c"
			for (cc=0; cc<3; cc++) 
			{
				tmp = a[cc][c];
				a[cc][c] = a[cc][rowMax];
				a[cc][rowMax] = tmp;
				tmp = b[cc][c];
				b[cc][c] = b[cc][rowMax];
				b[cc][rowMax] = tmp;
			}

			// Now everything we do is on row "c".
			// Set the max cell to 1 by dividing the entire row by that value
			tmp = a[c][c];
			for (cc=0; cc<3; cc++) 
			{
				a[cc][c] /= tmp;
				b[cc][c] /= tmp;
			}

			// Now do the other rows, so that this column only has a 1 and 0's
			for (row = 0; row < 3; row++) 
			{
				if (row != c) 
				{
					tmp = a[c][row];
					for (cc=0; cc<3; cc++) 
					{
						a[cc][row] -= a[cc][c] * tmp;
						b[cc][row] -= b[cc][c] * tmp;
					}
				}
			}

		}

		*this = b;
		isInvertible = true;

		return *this;
	}
	

	matrix44 &matrix44::invert(bool &isInvertible)
	{
		matrix44 a(*this);
		matrix44 b(IdentityMatrix44());

		unsigned int r, c;
		unsigned int cc;
		unsigned int rowMax; // Points to max abs value row in this column
		unsigned int row;
		float tmp;

		// Go through columns
		for (c=0; c<4; c++) 
		{

			// Find the row with max value in this column
			rowMax = c;
			for (r=c+1; r<4; r++) 
			{
				if (fabs(a[c][r]) > fabs(a[c][rowMax])) 
				{
					rowMax = r;
				}
			}

			// If the max value here is 0, we can't invert.  Return identity.
			if (a[rowMax][c] == 0.0F) 
			{
				isInvertible = false;
				return (identity());
			}

			// Swap row "rowMax" with row "c"
			for (cc=0; cc<4; cc++) 
			{
				tmp = a[cc][c];
				a[cc][c] = a[cc][rowMax];
				a[cc][rowMax] = tmp;
				tmp = b[cc][c];
				b[cc][c] = b[cc][rowMax];
				b[cc][rowMax] = tmp;
			}

			// Now everything we do is on row "c".
			// Set the max cell to 1 by dividing the entire row by that value
			tmp = a[c][c];
			for (cc=0; cc<4; cc++) 
			{
				a[cc][c] /= tmp;
				b[cc][c] /= tmp;
			}

			// Now do the other rows, so that this column only has a 1 and 0's
			for (row = 0; row < 4; row++) 
			{
				if (row != c) 
				{
					tmp = a[c][row];
					for (cc=0; cc<4; cc++) 
					{
						a[cc][row] -= a[cc][c] * tmp;
						b[cc][row] -= b[cc][c] * tmp;
					}
				}
			}

		}

		*this = b;
		
		isInvertible = true;

		return *this;
	}


	matrix33 InvertMatrix33(const matrix33 &m, bool &isInvertible) 
	{
		matrix33 ret(m);

		return ret.invert(isInvertible);
	}


	matrix44 InvertMatrix44(const matrix44 &m, bool &isInvertible) 
	{
		matrix44 ret(m);

		return ret.invert(isInvertible);
	}

	matrix44 Quaternion2Matrix(vector4 q)
	{
		matrix44 m = IdentityMatrix44();
		double sqw = q.w*q.w;
		double sqx = q.x*q.x;
		double sqy = q.y*q.y;
		double sqz = q.z*q.z;

		// invs (inverse square length) is only required if quaternion is not already normalised
		double invs = 1 / (sqx + sqy + sqz + sqw);
		m[0][0] = ( sqx - sqy - sqz + sqw)*invs; // since sqw + sqx + sqy + sqz =1/invs*invs
		m[1][1] = (-sqx + sqy - sqz + sqw)*invs;
		m[2][2] = (-sqx - sqy + sqz + sqw)*invs;

		double tmp1 = q.x*q.y;
		double tmp2 = q.z*q.w;
		m[0][1] = 2.0 * (tmp1 + tmp2)*invs;
		m[1][0] = 2.0 * (tmp1 - tmp2)*invs;

		tmp1 = q.x*q.z;
		tmp2 = q.y*q.w;
		m[0][2] = 2.0 * (tmp1 - tmp2)*invs;
		m[2][0] = 2.0 * (tmp1 + tmp2)*invs;
		tmp1 = q.y*q.z;
		tmp2 = q.x*q.w;
		m[1][2] = 2.0 * (tmp1 + tmp2)*invs;
		m[2][1] = 2.0 * (tmp1 - tmp2)*invs;      
		return m;
	}


	float matrix33::det() const
	{
		int tab[6][3] = {
			{0, 1, 2},
			{0, 2, 1},
			{1, 0, 2},
			{1, 2, 0},
			{2, 0, 1},
			{2, 1, 0}
			};
		float det = 0;
		mtxlib::matrix33 a(*this);
		for(int i = 0; i < 6; i++)
		{
			det += a[tab[i][0]][0] * a[tab[i][1]][1] * a[tab[i][2]][2] * (i % 2 == 0 ? 1 : -1);
		}
		return det;
	}


	float matrix44::det() const
	{
		int tab[24][4] = {
			{0, 1, 2, 3},
			{0, 1, 3, 2},
			{0, 2, 1, 3},
			{0, 2, 3, 1},
			{0, 3, 1, 2},
			{0, 3, 2, 1},
			{1, 0, 2, 3},
			{1, 0, 3, 2},
			{1, 2, 0, 3},
			{1, 2, 3, 0},
			{1, 3, 0, 2},
			{1, 3, 2, 0},
			{2, 0, 1, 3},
			{2, 0, 3, 1},
			{2, 1, 0, 3},
			{2, 1, 3, 0},
			{2, 3, 0, 1},
			{2, 3, 1, 0},
			{3, 0, 1, 2},
			{3, 0, 2, 1},
			{3, 1, 0, 2},
			{3, 1, 2, 0},
			{3, 2, 0, 1},
			{3, 2, 1, 0}
		};
		float det = 0;
		mtxlib::matrix44 a(*this);
		for(int i = 0; i < 24; i++)
		{
			det += a[tab[i][0]][0] * a[tab[i][1]][1] * a[tab[i][2]][2] * a[tab[i][3]][3] * (i % 2 == 0 ? 1 : -1);
		}
		return det;
	}
}

